package Movement;


import lejos.nxt.NXTRegulatedMotor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.navigation.DifferentialPilot;

public class NEPilot extends DifferentialPilot{

	public NEPilot(double leftWheelDiameter, double trackWidth, RegulatedMotor leftMotor, RegulatedMotor rightMotor) {
		super(leftWheelDiameter,trackWidth,leftMotor,rightMotor);

	}

	/**
	 * ----------------------------------------------------------------
	 * 								Attributes
	 * ----------------------------------------------------------------
	 */
	
	protected static NEPilot pilot;
	protected TouchSensor touch;
	
	/**
	 * ----------------------------------------------------------------
	 * 								Static constructor
	 * ----------------------------------------------------------------
	 */
	
	public static NEPilot newNEPilotWithWheelDiameterWidthAndLeftAndRightMotors(double wheelDiameter, double width, NXTRegulatedMotor leftMotor, NXTRegulatedMotor rightMotor) {
		pilot = new NEPilot(wheelDiameter, width, leftMotor, rightMotor);
		pilot.setRotateSpeed(50);		
		
		return pilot;

	}
	
	/*Prise en compte de la detection de choc*/
	public static NEPilot newNEPilotWithWheelDiameterWidthAndLeftAndRightMotorsAndTouchPort(double wheelDiameter, double width, NXTRegulatedMotor leftMotor, NXTRegulatedMotor rightMotor, SensorPort touchPort) {
		pilot = new NEPilot(wheelDiameter, width, leftMotor, rightMotor);
		pilot.setRotateSpeed(50);		
		pilot.setTouch(touchPort);
		return pilot;

	}
	
	

	/**
	 * ----------------------------------------------------------------
	 * 								Setters
	 * ----------------------------------------------------------------
	 */
	private void setTouch(SensorPort touchPort) {
		touch = new TouchSensor(touchPort);
		
	}
	
	
	/**
	 * ----------------------------------------------------------------
	 * 								Getters
	 * ----------------------------------------------------------------
	 */
	
	
	private TouchSensor touch() {
		return this.touch;
		
	}

	
	/**
	 * ----------------------------------------------------------------
	 * 								Movement
	 * ----------------------------------------------------------------
	 */
	
		
	
	/*
	public void turnRight(float angle)
	{
		//le robot ralenti
		setTravelSpeed(2);
		
		this.compassPilot.resetCartesianZero();
		float startPosition = this.compassPilot.getDegrees();
		float endPosition;
		
		//l'angle total sera sous les 359�
		if(startPosition<=(359-angle))
		{
			//l'angle � atteindre est l'angle de d�part + la rotation � effectuer
			endPosition = startPosition+angle;
			
			//le robot tourne jusqu'� atteindre l'angle voulu
			while(this.compassPilot.getDegrees()<endPosition)
			{
				rotateRight();
			}
		}
		//l'angle total passe au dessus des 359� et donc repasse par 0�
		else
		{
			//par ex: rotationAngle = 50� and startPosition= 340. 
			//EndPosition= 50 -(359- 340) = 31
			endPosition = angle -(359 -startPosition);
			
			//le robot tourne jusqu'� atteindre l'angle voulu
			while(this.compassPilot.getDegrees()!=endPosition)
			{
				rotateRight();
			}
		}
		forward();
		//le robot acc�l�re
		setTravelSpeed(5);
	}

	public void turnLeft(float angle)
	{
		//le robot ralenti pour tourner
		setTravelSpeed(2);
		
		this.compassPilot.resetCartesianZero();
		float startPosition = this.compassPilot.getDegrees();
		float endPosition;
	
		//l'angle actuel est sup�rieur � l'angle � r�aliser
		if(startPosition>=angle)
		{
			//l'angle � atteindre est l'angle de d�part - la rotation (vers 0)
			endPosition = startPosition-angle;
			
			//le robot tourne jusqu'� atteindre l'angle voulu
			while(this.compassPilot.getDegrees()>endPosition)
			{
				rotateLeft();
			}
		}
		else
		{
			//par ex: rotationAngle = 50� and startPosition= 40. 
			//EndPosition= 359 - (50 - 40) = 349
			endPosition = 359 - (angle -startPosition);
			
			//la robot tourne jusqu'� atteindre l'angle voulu
			while(this.compassPilot.getDegrees()!=endPosition)
			{
				rotateLeft();
			}
		}
		
		forward();
		
		//the robot speed
		setTravelSpeed(5);
	}*/

	/**
	 * ----------------------------------------------------------------
	 * 								Shock Detection
	 * ----------------------------------------------------------------
	 */
	
	public boolean shockDetection()
	{
		return this.touch.isPressed();
	}

	

}
